#include "cv.h"
#include "cxcore.h"
#include "highgui.h"
#include <cstdio>
#include <iostream>
using namespace std;
using namespace cv;

#define IMG_PIXEL1(img, i, j) ((uchar *)(img->imageData + i*img->widthStep))[j]
#define IMG_PIXEL3(img, i, j, c) ((uchar *)(img->imageData + i*img->widthStep))[j*img->nChannels + c]
#define IMG_PIXEL_ISBLACK(img, i, j) ((IMG_PIXEL3(img, i, j, 0)==0) && (IMG_PIXEL3(frame, i, j, 1)==0) && (IMG_PIXEL3(frame, i, j, 2)==0))

IplImage* segmentarColor(IplImage* frame, CvScalar color, float radio){
	// con esto nos evitamos reservar memoria cada vez que se llama a la funcion
	static IplImage* mascara=NULL;
	if(mascara==NULL){
		mascara=cvCreateImage(cvSize(frame->width,frame->height),frame->depth,frame->nChannels);
	}
	CvScalar blanco, negro;
	blanco.val[0]=blanco.val[1]=blanco.val[2]=255;
	negro.val[0]=negro.val[1]=negro.val[2]=0;
	
	CvScalar s;
	float dist;
	unsigned i, j;
	for(i=0; i<frame->width; i++){
		for(j=0; j<frame->height; j++){
			s=cvGet2D(frame, j, i);
			dist=abs(color.val[0]-s.val[0])+abs(color.val[1]-s.val[1])+abs(color.val[2]-s.val[2]);
			if(dist<=radio){
				cvSet2D(mascara, j, i, blanco);
			}else{
				cvSet2D(mascara, j, i, negro);
			}
		}
	}
	return mascara;
}


IplImage* segmentarColorHSV(IplImage* frame, CvScalar color, float radio){
	static IplImage* canalH=NULL;
	static IplImage* mascara=NULL;
	if(canalH==NULL){
		canalH=cvCreateImage(cvSize(frame->width,frame->height),frame->depth,1);
		mascara=cvCreateImage(cvSize(frame->width,frame->height),frame->depth,3);
	}
	// convertimos a HSV y guardamos el canal H
	cvCvtColor(frame, frame, CV_RGB2HSV); 
	cvSplit(frame, NULL, NULL, canalH, NULL);
	
	CvScalar blanco, negro, s;
	blanco.val[0]=blanco.val[1]=blanco.val[2]=255;
	negro.val[0]=negro.val[1]=negro.val[2]=0;
	
	float dist;
	unsigned i, j;
	for(i=0; i<frame->width; i++){
		for(j=0; j<frame->height; j++){
			s=cvGet2D(frame, j, i);
			dist=abs(color.val[0]-s.val[0]);
			if(dist<=radio){
				cvSet2D(mascara, j, i, blanco);
			}else{
				cvSet2D(mascara, j, i, negro);
			}
		}
	}
	cvCvtColor(frame, frame, CV_HSV2RGB);
	return mascara;
}


IplImage* MaskFromObject(IplImage* frame, CvScalar color, float radio){
return NULL;
}

// equalizacion por instograma in place
void HistoEqHSI(IplImage* frame){
	// con esto nos evitamos reservar memoria cada vez que se llama a la funcion
	// OJO que los dejamos a todos en NULL!!!!!!!!!!
	static IplImage* canalH=NULL;
	static IplImage* canalS=NULL;
	static IplImage* canalV=NULL;
	if(canalV==NULL){
		canalV=cvCreateImage(cvSize(frame->width,frame->height),frame->depth,1);
	}
	// convertimos a HSV y guardamos el canal V
	cvCvtColor(frame, frame, CV_RGB2HSV); 
	cvSplit(frame, canalH, canalS, canalV, NULL);
	
	cvEqualizeHist(canalV, canalV);
	cvMerge(canalH, canalS, canalV, NULL, frame);
	cvCvtColor(frame, frame, CV_HSV2RGB);
}


void Normalize(IplImage* frame){
	// con esto nos evitamos reservar memoria cada vez que se llama a la funcion
	// OJO que los dejamos a todos en NULL!!!!!!!!!!
	static IplImage* canalH=NULL;
	static IplImage* canalS=NULL;
	static IplImage* canalV=NULL;
	static IplImage* canalVNormalizado=NULL;
	if(canalV==NULL){
		canalH=cvCreateImage(cvSize(frame->width,frame->height),frame->depth,1);
		canalS=cvCreateImage(cvSize(frame->width,frame->height),frame->depth,1);
		canalV=cvCreateImage(cvSize(frame->width,frame->height),frame->depth,1);
		canalVNormalizado=cvCreateImage(cvSize(frame->width,frame->height), frame->depth, 1);
	}
	// convertimos a HSV y guardamos el canal V
	cvCvtColor(frame, frame, CV_RGB2HSV); 
	cvSplit(frame, canalH, canalS, canalV, NULL);
	
	cvNormalize(canalV, canalVNormalizado, 0,  255, CV_MINMAX);
	cvMerge(canalH, canalS, canalVNormalizado, NULL, frame);
	cvCvtColor(frame, frame, CV_HSV2RGB);
}






#include <map>
IplImage*  Etiquetar(IplImage *frame){
	static IplImage* etiquetas=NULL;
	if(etiquetas==NULL){
		etiquetas=cvCreateImage(cvSize(frame->width,frame->height),frame->depth, IPL_DEPTH_32F);
	}
	map<int,int> equivalencias;
	// limpia la imagen
	cvSet(etiquetas, cvScalar(0));
	int totales=0;
	unsigned i, j, k, l, nObjects=0;
	// recorre toda la imagen
	for(i=1; i<frame->height-1; i++){ // los limites son para no tener problemas con vecinos
		for(j=1; j<frame->width-1; j++){
			// comparamos el pixel de la imagen para saber si es distinto de 0...
			if(!IMG_PIXEL_ISBLACK(frame, i, j)){
				if(IMG_PIXEL1(etiquetas, i, j)==0){
					IMG_PIXEL1(etiquetas, i , j)=++nObjects; totales++;

					// recorre la vecindad
					for(k=i; k<i+1; k++){
						for(l=j; l<j+1; l++){
							if(k==i && l==j) continue; 
							if(!IMG_PIXEL_ISBLACK(frame, k, l)){
								if(IMG_PIXEL1(etiquetas, k , l)==0){
									// si el vecino esta sin etiquetar, lo eqtiquetamos
									IMG_PIXEL1(etiquetas, k , l)=nObjects;
								}else{
									// sino, ponemos en la tabla de equivalencias
									equivalencias[nObjects]=IMG_PIXEL1(etiquetas, k , l);
									totales--;
								}
							}
						}
					}
					
				}
			}
		}
	}
	cout<<"tot: "<<totales<<endl;
	
	
	// segunda pasada, aplica las equivalencias
	map<int, int>::iterator p;
	for(i=1; i<frame->height-1; i++){ // los limites son para no tener problemas con vecinos
		for(j=1; j<frame->width-1; j++){
			if(IMG_PIXEL1(etiquetas, i, j)!=0 && (p=equivalencias.find(IMG_PIXEL1(etiquetas, i, j)))!=equivalencias.end()){
				IMG_PIXEL1(etiquetas, i, j)=p->second;
			}
		}
	}
	
	return etiquetas;
}






// Variables globales
bool visualizaColor = true;
CvScalar colorSegmentacion;





void onMouse(int event, int x, int y, int flags, void *param){
	int tamVentana=7, i, j, tamVentana_2=tamVentana/2;
	CvScalar s;
	
	switch(event) {
		/* left button down */
//	case CV_EVENT_RBUTTONDOWN:
//		fprintf(stdout, "Left button down (%d, %d).\n", x, y);
//		break;
//		
//		/* right button down */
		
	case CV_EVENT_LBUTTONDOWN:
		colorSegmentacion=cvGet2D((IplImage* )param, y, x);
	break;
//		
//		/* mouse move */
//	case CV_EVENT_MOUSEMOVE:
//		/* draw a rectangle */
//		mousex=x;
//		mousey=y;
//		break;
	}
}










int main( int argc, char** argv ){
	// imagenes
	IplImage* frameOrig = 0;
	IplImage* frameFinal = 0;
	IplImage* frameBN = 0;
	IplImage* blureada = 0;
	IplImage* mascara1 = 0;
	IplImage* mascara2 = 0;
	IplImage* mascara3 = 0;
	IplImage* etiquetas = 0;
	IplImage* multip = 0;
	
	
	// otras variables
	int radio1=50, radio2=50;
	int ancho, alto;
	int tecla;
	

	
	// En OpenCV todas las variables son punteros. Algunas funciones
	// crean el espacio necesario y devuelven la dirección de memoria.
	//
	// En este ejemplo cvCreateCameraCapture() crea el espacio y se lo
	// asigna a "capture". Nunca se debe sobreescribir en "capture".
	// Devuelve 0 si no se detecta cámara.
	CvCapture *capture = cvCreateCameraCapture(CV_CAP_ANY);

	if ( !capture ) {
		printf("ERROR: capture is NULL \n" );
		return -1;
	}
	
	// Establecer la resolución de trabajo (aceptados por la camara).
	//ancho = 352; alto = 288;
	ancho = 1600; alto = 1200;
	cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, ancho);
	cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, alto);
	
	/* Video capture property: cvSetCaptureProperty()
	CV_CAP_PROP_POS_MSEC
	CV_CAP_PROP_POS_FRAME
	CV_CAP_PROP_POS_AVI_RATIO
	CV_CAP_PROP_FRAME_WIDTH
	CV_CAP_PROP_FRAME_HEIGHT
	CV_CAP_PROP_FPS
	CV_CAP_PROP_FOURCC
	CV_CAP_PROP_FRAME_COUNT
	*/
	
	
	// Inicializar imagenes:
	//
	// "frameOrig" se utilizara para saber cual es el fotograma actual. Sus
	// datos son generados en un area de memoria por el "capture", asi que
	// no se tiene que inicializar con nada, solo se apunta.
	frameOrig = cvQueryFrame( capture );
	if ( !frameOrig ) {
		cout<<"ERROR: no se detecto imagen desde la webcam...\n";
		return 1;
	}
	const int accum_size=5;
	vector<IplImage*> accum_vec(accum_size);
	for(unsigned i=0; i<accum_size; i++){
		accum_vec[i]=cvCreateImage(cvSize(frameOrig->width,frameOrig->height),frameOrig->depth,frameOrig->nChannels);
	}
	
	
	// Crear una imagen del mismo tamaño que el frameOrig a extraer de la
	// camara pero de 1 canal (escala de grises).
	frameBN = cvCreateImage(cvSize(frameOrig->width,frameOrig->height),frameOrig->depth,3);
	blureada = cvCreateImage(cvSize(frameOrig->width,frameOrig->height),frameOrig->depth,frameOrig->nChannels);
	frameFinal = cvCreateImage(cvSize(frameOrig->width,frameOrig->height),frameOrig->depth,frameOrig->nChannels);
	multip = cvCreateImage(cvSize(frameOrig->width,frameOrig->height),frameOrig->depth,frameOrig->nChannels);
	mascara2 = cvCreateImage(cvSize(frameOrig->width,frameOrig->height),frameOrig->depth,1);
	mascara1 = cvCreateImage(cvSize(frameOrig->width,frameOrig->height),frameOrig->depth,1);

	
	
	// Las ventanas se identifican por su título
    cvNamedWindow( "OpenCV Simple", CV_WINDOW_AUTOSIZE );
	cvSetMouseCallback("OpenCV Simple", onMouse, frameOrig);
	
	// crear un slider para la variable radio...
	cvCreateTrackbar("RadioRGB", "OpenCV Simple", &radio1, 512, NULL);
	cvCreateTrackbar("RadioHSV", "OpenCV Simple", &radio2, 512, NULL);
	
	while ( true ) {
		// Obtener un frame
		frameOrig = cvQueryFrame(capture);
		if ( !frameOrig ) {
			cout<<"ERROR: no se detecto imagen desde la webcam...\n";
			continue;
		}
		
		

		
		if(visualizaColor){
			cvShowImage("OpenCV Simple", frameOrig);
		} else {
//			HistoEqHSI(frameOrig);
//			cvShowImage("OpenCV Simple", frameOrig);
//			cvSmooth(frameOrig, blureada, CV_MEDIAN, 5, 5); // filtro de mediana
	
//			mascara=segmentarColorHSV(blureada, colorSegmentacion, radio2);
			//mascara2=segmentarColor(blureada, colorSegmentacion, radio1);
			//cvAnd(mascara, mascara2, frameBN);
			//cvAnd(frameOrig, frameBN, frameFinal);
			//etiquetas=Etiquetar(frameFinal);
			//cvEqualizeHist(etiquetas, etiquetas);
//			cvAnd(blureada, mascara, frameFinal);
			
			cvReleaseImage(&accum_vec[0]);
			for(unsigned i=0; i<accum_size-1; i++){
				accum_vec[i]=accum_vec[i+1];
			}
			accum_vec[accum_size-1]=cvCreateImage(cvSize(frameOrig->width,frameOrig->height),frameOrig->depth,frameOrig->nChannels);
			cvCopy(frameOrig, accum_vec[accum_size-1]);
			//cvConvertScale(accum_vec[accum_size-1], accum_vec[accum_size-1], 1.0/accum_size);
			
			CvScalar s, r;
			unsigned i, j;
			cvZero(frameFinal);
			cvZero(multip);
			for(unsigned k=0; k<accum_size; k++){
				cvConvertScale(accum_vec[k], multip, float(1)/(accum_size));
				cvAdd(frameFinal,  multip, frameFinal);
			}
			cvAbsDiff(frameOrig, frameFinal, frameFinal);
			//Normalize(frameFinal);
			cvCvtColor(frameFinal, frameFinal, CV_HSV2RGB);
			
			
			cvThreshold(frameFinal, frameFinal, radio2, 255, CV_THRESH_BINARY);
			cvCvtColor(frameFinal, mascara2 ,CV_BGR2GRAY);
			cvSmooth(mascara2, mascara1, CV_MEDIAN, 7, 7); // filtro de mediana
			cvCvtColor(mascara2 , blureada,CV_GRAY2RGB);
			cvAdd(frameFinal,  blureada, frameFinal);
			cvShowImage("OpenCV Simple", frameFinal);
		}
		
		
		
		// Milisegundos de espera para reconocer la tecla presionada
		tecla = cvWaitKey(10);
		if(((char)tecla & 255)==27) break;

		switch ((char)tecla){
			case 'c':
			case 'C':
			visualizaColor=!visualizaColor; break;
			
		}
		
	} // while
	
	
	// Liberar todos los recursos que hayamos creado explicitamente
	// mediante cualquier cvCreate....()
    cvReleaseCapture( &capture ); // memoria asignada para la cámara
	cvReleaseImage(&frameBN); // solo esta, no "frame"
    cvDestroyWindow("OpenCV Simple"); // recursos de la ventana

    return 0;
}
